Control device

ABSTRACT

A model prediction control part of a control device includes an obstacle avoidance control unit that operates when there are a plurality of actual obstacles to be avoided. The obstacle avoidance control unit decides the position of a virtual obstacle from the positions of the plurality of actual obstacles acquired by an acquisition part so as to be positioned between the plurality of actual obstacles, and performs model prediction control by using, as the stage cost, the addition result of a standard cost and a virtual obstacle evaluation term for which a prescribed function, which uses, as parameters, at least the position of the virtual obstacle and the position of a moving body, is multiplied by a virtual obstacle weight. Using this configuration, when the moving body is caused to follow with respect to a target trajectory by the model prediction control, a collision with an obstacle can be avoided suitably.

CROSS-REFERENCE TO RELATED APPLICATION

This application is a 371 application of the International PCTapplication serial no. PCT/JP2019/038202, filed on Sep. 27, 2019, whichclaims the priority benefit of Japanese Patent Application No.2018-182621, filed on Sep. 27, 2018. The entirety of each of theabove-mentioned patent applications is hereby incorporated by referenceherein and made a part of this specification.

BACKGROUND Technical Field

The present invention relates to a control device which controls adriving device that moves a moving body.

Related Art

In order to move a moving body without colliding with an obstacle, in anevaluation function for deciding a route of the moving body, anevaluation term for not allowing the moving body to approach theobstacle (a penalty term, hereinafter referred to as an obstacleevaluation term) is included for each obstacle (for example, see Patentliteratures 1 and 2).

LITERATURE OF RELATED ART Patent Literature

Patent literature 1: Japanese Patent Laid-Open No. 2011-186878

Patent literature 2: Japanese Patent Laid-Open No. 2014-130495

SUMMARY Problems to be Solved

If the evaluation function including the obstacle evaluation term foreach obstacle is used, the moving body can be moved without collidingwith the obstacle. However, if the evaluation function including theobstacle evaluation term is simply used, a case in which the collisionwith the obstacle cannot be avoided suitably may occur, for example, acase in which the moving body comes into a stationary state, or a casein which an obstacle avoidance route of the moving body makes a largedetour.

The present invention is completed in view of the above problems, andaims to provide a control device which can suitably avoid a collisionwith an obstacle when the moving body is caused to follow with respectto a target trajectory by model prediction control.

Means to Solve Problems

In order to achieve the above aim, a control device which controls adriving device that moves a moving body according to an aspect of thepresent invention includes: an acquisition part that acquires a positionof each actual obstacle which may obstruct movement of the moving bodyalong a target trajectory; and a model prediction control part thatmoves the moving body along the target trajectory by calculating acontrol input and supplying the control input to the driving devicebased on a prediction model, determines the presence/absence of anactual obstacle to be avoided according to information from theacquisition part, and uses a standard cost as the stage cost when thereis no actual obstacle to be avoided, wherein the control input minimizesa value of an evaluation function including a sum of stage costs in apredicted section of a prescribed time width, and the prediction modelis used for predicting the position of the moving body from the controlinput to the driving device. Besides, the model prediction control partof the control device includes an obstacle avoidance control means thatoperates when there are a plurality of actual obstacles to be avoided,decides the position of a virtual obstacle from the positions of theplurality of actual obstacles acquired by the acquisition part so as tobe positioned between the plurality of actual obstacles, and calculatesthe control input that minimizes the evaluation function by using, asthe stage cost, the addition result of the standard cost and a virtualobstacle evaluation term for which a prescribed function, which uses, asparameters, at least the position of the virtual obstacle and theposition of the moving body, is multiplied by a virtual obstacle weight.

That is, the control device has a configuration in which a virtualobstacle is assumed to exist and the control input is calculated in acase in which the collision between the moving body and the obstaclecannot be avoided suitably if the evaluation function including theobstacle evaluation term is simply used (a case in which two obstaclesexist in a way of clamping the target trajectory, or the like). Thus,according to the control device, even in the case in which the collisionbetween the moving body and the obstacle cannot be avoided suitably ifthe evaluation function including the obstacle evaluation term is simplyused, the collision between the moving body and the obstacle can beavoided suitably.

The obstacle avoidance control means may be a means that uses, as thestage cost, the addition result of the standard cost, the virtualobstacle evaluation term, and an actual obstacle evaluation term forwhich the prescribed function is multiplied by the weight for eachactual obstacle to be avoided; or may be a means that uses, as the stagecost, the addition result of the standard cost and the virtual obstacleevaluation term.

When the latter means is used as the obstacle avoidance control means, acapability of (deciding the value of the virtual obstacle weight in away that when the operation is started, a calculation result of thevirtual obstacle evaluation term at that time point matches the sum ofvalues at that time point of the actual obstacle evaluation terms forwhich the prescribed function is multiplied by the weight for eachactual obstacle to be avoided) is preferably applied to the obstacleavoidance control means.

In addition, when there are two or three actual obstacles to be avoided,the obstacle avoidance control means may decide the position of thevirtual obstacle to a position which is equidistant from the two orthree actual obstacles are equal and has the distance shortest to eachactual obstacle.

In addition, in the control device, a configuration in which (the modelprediction control part further includes a determination means whichdetermines whether or not the moving body comes into a speed reducingstate, and the obstacle avoidance control means of the model predictioncontrol part starts the operation when the moving body is determined tocome into the speed reducing state under a situation that there are aplurality of actual obstacles to be avoided by the determination means)may be used. Moreover, as the determination means, for example, thefollowing means can be used: a means which determines that the movingbody comes into the speed reducing state when an absolute value of adifference between a current value and a previous value of the minimumvalue of the evaluation function, or the absolute value of thedifference between a current value and a previous value of the positionof the moving body is less than a prescribed threshold value; or a meanswhich determines that the moving body comes into the speed reducingstate when the absolute value of the difference between the currentvalue and the previous value of the minimum value of the evaluationfunction, or the absolute value of the difference between the currentvalue and the previous value of the position of the moving body is lessthan a prescribed threshold value for a prescribed number of timescontinuously.

In addition, as the obstacle avoidance control means, a means may beused which uses as, the prescribed function, a function in which achange rate of the virtual obstacle evaluation term in an arrangementdirection of the two actual obstacles is smaller than that in anotherdirection when there are two actual obstacles to be avoided.

The model prediction control part may take all of the actual obstaclesof which the positions are acquired by the acquisition part as theobstacles to be avoided. In addition, when the positions of theplurality of actual obstacles are acquired by the acquisition part, themodel prediction control part may calculate, for each of the pluralityof actual obstacles, an evaluation value indicating a probability inwhich a collision with the moving body can occur, and may decide whichof the plurality of actual obstacles is the actual obstacle to beavoided based on the evaluation value calculated for each actualobstacle.

Effect

According to the present invention, when the moving body is caused tofollow with respect to the target trajectory by the model predictioncontrol, a collision between the moving body and the obstacle can beavoided suitably.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is an illustration diagram of a use form of a control deviceaccording to an embodiment of the present invention.

FIG. 2 is an illustration diagram of a schematic configuration of thecontrol device according to the embodiment.

FIG. 3 is an illustration diagram of a control result example of thecontrol device according to the embodiment.

FIG. 4 is an illustration diagram of a capability of the control deviceaccording to the embodiment.

FIG. 5 is an illustration diagram of a control result example of thecontrol device according to the embodiment.

DESCRIPTION OF THE EMBODIMENTS

Hereinafter, an embodiment of the present invention is described withreference to the drawings.

FIG. 1 shows a use form of a control device 10 according to anembodiment of the present invention, and FIG. 2 shows a schematicconfiguration of the control device 10 according to the embodiment.

The control device 10 (FIG. 1 ) according to the embodiment is a devicewhich is used for respectively controlling, by a model predictioncontrol, a plurality of (one in the diagram) motors 21 driving aspecific load device 22 and is generally referred to as a servoamplifier, a servo driver, or the like. Moreover, the load device 22 isan arm of an industrial robot, a carrying device, or the like.Hereinafter, a part including the plurality of motors 21 controlled bythe control device 10 and the load device 22 driven by each of themotors 21 is referred to as a control object 20.

As shown in FIG. 2 , the control device 10 is configured to function asan acquisition part 11 and a model prediction control part 12.

The acquisition part 11 is a unit which acquires a position (centerposition) and a size of each actual obstacle existing in the surroundingof a moving body in the control object 20 (a hand of a robot or thelike, hereinafter simply referred to as a moving body) by analyzing animaging result obtained by a camera 30. Moreover, the camera 30 maycapture images only in a prescribed range in an advancing direction ofthe moving body. In addition, the acquisition part 11 may acquire theposition and the size only of each actual obstacle existing in theprescribed range in the advancing direction of the moving body (in otherwords, each actual obstacle to be avoided).

The model prediction control part 12 is a unit which controls, based onthe position and the size of each obstacle acquired by the acquisitionpart 11 and information from the control object 20 (a position of themoving body and the like), the control object 20 by the model predictioncontrol in order that the moving body avoids each obstacle and movesalong a target trajectory instructed from a programmable logiccontroller (PLC) 31.

The control performed by the model prediction control part 12 is thecontrol in which (an input u minimizing a value of an evaluationfunction J described below is calculated using a prediction model of thecontrol object 20 and based on a state x of the control object to supplyto the control object 20).

[Mathematical Formula 1]

$\begin{matrix}{J = {{\varphi\left( {x\left( {t + T} \right)} \right)} + {\int_{0}^{t + T}{{L\left( {{x(\tau)},{u(\tau)}} \right)}d\tau}}}} & \left( {{Formula}1} \right)\end{matrix}$Moreover, a first term on the right side of Formula 1 denotes a terminalcost, and L in Formula 1 denotes a stage cost. In addition, T denotes atime width which is previously set as a length of a predicted section inthe model prediction control.

That is, the control performed by the model prediction control part 12is basically the same as the model prediction control which is generallyperformed.

Herein, the model prediction control part 12 is configured to change thestage cost L used for the calculation of the J value depending on thesituation.

Taking the above as the premise, hereinafter, a capability of the modelprediction control part 12 is described specifically. Moreover, thecontrol device 10 is a device which can also move the moving body in athree-dimensional space. However, hereinafter, the moving body is set tobe moved on a plane surface for convenience of description.

The model prediction control part 12 periodically determines thepresence/absence of the actual obstacle to be avoided based on theposition and the size of each actual obstacle acquired by theacquisition part 11. More specifically, when no position and size of theactual obstacle is acquired by the acquisition part 11 (when there is noactual obstacle), the model prediction control part 12 determines thatthere is no actual obstacle to be avoided. In addition, when thepositions and the sizes of one or more actual obstacles are acquired bythe acquisition part 11, based on the position and the size of eachactual obstacle, the model prediction control part 12 specifies actualobstacles, which satisfy a condition in which a value of an actualobstacle evaluation term L_(k) defined by the following Formula 2 isequal to or greater than a predetermined value, from the actualobstacles of which the positions and the sizes are acquired by theacquisition part 11. Then, the model prediction control part 12determines that each actual obstacle which has been specified is theactual obstacle to be avoided.[Mathematical formula 2]L _(k) =g _(k) exp(−√{square root over ((x ₁ −x _(ok))²+(y ₁ −y_(ok))²)})  (Formula 2)

In the above Formula 2, a suffix k denotes a number which begins from 1and is assigned for each actual obstacle that satisfies the abovecondition. x₁ and y₁ denote the position of the moving body (x and ycoordinates), and x_(ok) and y_(ok) denote the position of an actualobstacle k (x and y coordinates). In addition, g_(k) denotes a valuewhich is decided according to the size of the actual obstacle k.

Moreover, the model prediction control part 12 is configured to selectthree actual obstacles in descending order of values of the actualobstacle evaluation terms and determines that each of the selectedactual obstacles is the actual obstacle to be avoided when there arefour or more actual obstacles which satisfy the above condition. Thus,the maximum value of the reference character k is 3.

In addition, the model prediction control part 12 is configured toperiodically determine whether or not the moving body comes into a speedreducing state. The determination processing may determine that themoving body comes into the speed reducing state when an absolute valueof a difference between a current value and a previous value of theposition of the moving body is less than a prescribed position thresholdvalue; or may determine that the moving body comes into the speedreducing state when the absolute value of the difference between acurrent value and a previous value of the minimum value of thecalculated J value is less than a prescribed threshold value of the Jvalue. In addition, in order to prevent erroneous detection, the abovedetermination processing may be set to be a processing which determinesthat the moving body comes into the speed reducing state when theabsolute value of the difference between the current value and theprevious value of the position of the moving body, or the absolute valueof the difference between the current value and the previous value ofthe minimum value of the J value, is less than a threshold value for aprescribed number of times continuously.

As described above, the model prediction control part 12 periodicallydetermines whether or not the moving body comes into the speed reducingstate and periodically determines the presence/absence of the actualobstacle to be avoided, and the model prediction control part 12 alsoperforms a processing of determining which of the first to the thirdconditions described below is satisfied based on both of thedetermination results.

-   -   First condition: there is no actual obstacle to be avoided.    -   Second condition: there are one or more actual obstacles to be        avoided, and the third condition is not satisfied.    -   Third condition: under a situation that there are a plurality of        actual obstacles to be avoided, the moving body comes into a        speed reducing state.

Besides, when the first condition is satisfied (when there is no actualobstacle to be avoided), the model prediction control part 12 performs afirst model prediction control that is a model prediction control usinga standard cost L₀ described below as a stage cost L.

[Mathematical formula 3]

$L_{0} = {\frac{1}{2}\left( {{\left( {{x{ref}} - x} \right)^{T}{Q\left( {{x{ref}} - x} \right)}} + {u^{T}Qu}} \right)}$

Moreover, xref denotes a target state amount at a certain time in apredicted section, x denotes a state amount at the same time, and udenotes a control input at the same time. In addition, Q denotes acoefficient (weight coefficient) indicating a weight of the state amountin the stage cost, and R denotes a coefficient (weight coefficient)indicating a weight of the control input.

In addition, when the second condition is satisfied, the modelprediction control part 12 performs a second model prediction controlwhich is model prediction control using, as the stage cost L, thefollowing value, that is, a sum of the standard cost L₀ and the actualobstacle evaluation term L_(k) of each actual obstacle.

[Mathematical formula 4]

$L = {L_{0} + {\sum\limits_{k = 1}^{kmax}L_{k}}}$

Moreover, kmax in the above formula denotes the total number of theactual obstacles to be avoided (≤3 in the embodiment).

In addition, when the third condition is satisfied, firstly, the modelprediction control part 12 decides a position which is equidistant fromthe two or three actual obstacles to be avoided and has the shortestdistance to each actual obstacle. Next, the model prediction controlpart 12 calculates a weight g_(p) in which a value of the virtualobstacle evaluation term described below is the sum of the value of theactual obstacle evaluation term of each actual obstacle.L _(p) =g _(p) exp(−√{square root over ((x ₁ −x _(pk))²+(y ₁ −y_(pk))²)})  [Mathematical formula 5]Moreover, x_(pk) and y_(pk) denote the position of the virtual obstaclewhich is decided (x and y coordinates).

Specifically, for example, when there are two actual obstacles to beavoided, the model prediction control part 12 calculates the g_(p)according to the following formula by using values V₁ and V₂ of actualobstacle evaluation terms L₁ and L₂ at that time point (in a case inwhich the moving body is in the current position).g _(p)=(V ₁ +V ₂)exp(√{square root over ((x ₁ −x _(pk))²+(y ₁ −y_(pk))²)})  [Mathematical Formula 6]

Besides, the model prediction control part 12 starts a third modelprediction control which is a model prediction control using a sum ofthe baseline cost L₀ and a virtual obstacle evaluation term L_(p) as thestage cost L.

As described above, with respect to the control device 10, a capabilityis applied of performing the model prediction control using anevaluation function in which the obstacle evaluation term of each actualobstacle is replaced by the virtual obstacle evaluation term (the thirdmodel prediction control) when the moving body comes into the speedreducing state under the situation that there are the plurality ofactual obstacles to be avoided. In this case, if the evaluation functionincluding the obstacle evaluation term is simply used, a problem may begenerated easily, for example, a problem that the moving body comes intoa stationary state, or a problem that an obstacle avoidance route of themoving body makes a large detour. Besides, according to the third modelprediction control, as shown by a solid line in FIG. 3 , the moving bodycan be moved in a form that the above troubles do not occur. Moreover,in FIG. 3 , obs 1 and obs 2 represent actual obstacles to be avoided,and pobs represents a virtual obstacle. In addition, x denotes a targetposition group which predetermines the target trajectory (a command froma PCS 32).

In addition, the control device 10 is configured to treat only an actualobstacle having a large value of the obstacle evaluation term as theactual obstacle to be avoided. Thus, for example, as shown in FIG. 4 ,even when there are four actual obstacles obs 1 to obs 4, the actualobstacles obs 3 and obs 4 having small values of the obstacle evaluationterms are not treated as the actual obstacles to be avoided. Therefore,as shown in FIG. 5 , even when there are a great number of actualobstacles (obs 1 to obs 4 in the diagram), the avoidance route of theactual obstacle can also be prevented from excessively making a largedetour.

<Deformation Form>

With regard to the control device 10 according to the embodimentdescribed above, various deformations can be made. For example, duringthe third model prediction control, the following stage cost L may beused.

[Mathematical formula 7]

$\begin{matrix}{L = {L_{0} + {\sum\limits_{k = 1}^{kmax}L_{k}} + L_{p}}} & \left( {{Formula}3} \right)\end{matrix}$

Moreover, when the above stage cost L is used, the g_(p) value can be avalue (for example, a value which is previously set) smaller than theabove value.

In addition, during the third model prediction control, a calculationmay be performed in which each L_(k) value in the above Formula 3 is setto “0”.

In addition, the actual obstacle evaluation term and the virtualobstacle evaluation term may be functions in which the values becomegreater as the distances to the moving body become shorter (in otherwords, a probability of the collision with the moving body becomeshigher). Thus, a function different from the above may be used as eachevaluation term. In addition, in order to prevent the avoidancetrajectory from excessively making a large detour (in order to shorten aperiod in which the moving body is away from the target trajectory),when there are two actual obstacles to be avoided, as the virtualobstacle evaluation term, a function may be used in which a change ratein an arrangement direction of the two actual obstacles is smaller thanthat in another direction.

The control device 10 can also be deformed to a device which usesdifferent actual obstacle evaluation terms when selecting an actualobstacle to be an avoided object and when performing actual modelprediction control. In addition, it is natural that, for example, thecamera 30 may be a light detection and ranging (LIDAR) or the like, andthe control device 10 may be deformed to a device which controls amovement direction of a vehicle.

<Additions>

A control device (10), which controls a driving device (21) that moves amoving body, including:

an acquisition part (11) that acquires a position of each actualobstacle which may obstruct movement of the moving body along a targettrajectory; and

a model prediction control part (12) that moves the moving body alongthe target trajectory by calculating a control input and supplying thecontrol input to the driving device based on a prediction model,determines the presence/absence of an actual obstacle to be avoidedaccording to information from the acquisition part, and uses a standardcost as the stage cost when there is no actual obstacle to be avoided,wherein the control input minimizes a value of an evaluation functionincluding a sum of stage costs in a predicted section of a prescribedtime width, and the prediction model is used for predicting the positionof the moving body from the control input to the driving device, andwhereinthe model prediction control part (12) includesan obstacle avoidance control means that operates when there are aplurality of the actual obstacles to be avoided, decides the position ofa virtual obstacle from the positions of the plurality of actualobstacles acquired by the acquisition part so as to be positionedbetween the plurality of actual obstacles, and calculates the controlinput that minimizes the evaluation function by using, as the stagecost, an addition result of the standard cost and a virtual obstacleevaluation term for which a prescribed function, which uses, asparameters, at least the position of the virtual obstacle and theposition of the moving body, is multiplied by a virtual obstacle weight.

What is claimed is:
 1. A control device which controls a driving devicethat moves a moving body, comprising: a processor, configured to acquirea position of each of actual obstacles which may obstruct movement ofthe moving body along a target trajectory; and perform a modelprediction control to move the moving body along the target trajectoryby calculating a control input and supplying the control input to thedriving device based on a prediction model, determine presence/absenceof the actual obstacle to be avoided according to information acquiredfrom the processor, and use a standard cost as a stage cost when thereis no actual obstacle to be avoided, wherein the control input isconfigured to minimize a value of an evaluation function comprising asum of stage costs in a predicted section of a prescribed time width,and the prediction model is configured to be used for predicting aposition of the moving body from the control input to the drivingdevice, and wherein the model prediction control is further configuredto perform an obstacle avoidance control that is configured to operatewhen there are a plurality of the actual obstacles to be avoided, decidea position of a virtual obstacle from the positions of the plurality ofactual obstacles acquired by processor so as to be positioned betweenthe plurality of actual obstacles, and calculate the control input thatis configured to minimize the evaluation function by using, as the stagecost, an addition result of the standard cost and a virtual obstacleevaluation term for which a prescribed function, which uses, asparameters, at least the position of the virtual obstacle and theposition of the moving body, is multiplied by a virtual obstacle weight.2. The control device according to claim 1, wherein the obstacleavoidance control is configured to use, as the stage cost, an additionresult of the standard cost, the virtual obstacle evaluation term, andan actual obstacle evaluation term for which the prescribed function ismultiplied by the weight for each of the actual obstacles to be avoided.3. The control device according to claim 2, wherein when there are twoor three actual obstacles to be avoided, the obstacle avoidance controlis configured to decide the position of the virtual obstacle to aposition which is equidistant from the two or three actual obstacles andhas the shortest distance to each of the actual obstacles.
 4. Thecontrol device according to claim 2, wherein the model predictioncontrol is further configured to determine whether or not the movingbody comes into a speed reducing state, and the obstacle avoidancecontrol is configured to start operations when the moving body isdetermined to come into the speed reducing state under a situation thatthere are the plurality of actual obstacles to be avoided by the modelprediction control.
 5. The control device according to claim 1, whereinthe obstacle avoidance control is configured to use, as the stage cost,an addition result of the standard cost and the virtual obstacleevaluation term.
 6. The control device according to claim 5, wherein theobstacle avoidance control is configured to decide a value of thevirtual obstacle weight in a way that when starting operations, acalculation result of the virtual obstacle evaluation term at that timepoint matches the sum of values at that time point of the actualobstacle evaluation terms for which the prescribed function ismultiplied by the weight for each of the actual obstacles to be avoided.7. The control device according to claim 6, wherein When there are twoactual obstacles to be avoided, the obstacle avoidance control isconfigured to use, as the prescribed function, a function in which achange rate of the virtual obstacle evaluation term in an arrangementdirection of the two actual obstacles is smaller than that in anotherdirection.
 8. The control device according to claim 5, wherein whenthere are two or three actual obstacles to be avoided, the obstacleavoidance control is configured to decide the position of the virtualobstacle to a position which is equidistant from the two or three actualobstacles and has the shortest distance to each of the actual obstacles.9. The control device according to claim 5, wherein the model predictioncontrol is further configured to determine whether or not the movingbody comes into a speed reducing state, and the obstacle avoidancecontrol is configured to start operations when the moving body isdetermined to come into the speed reducing state under a situation thatthere are the plurality of actual obstacles to be avoided by the modelprediction control.
 10. The control device according to claim 1, whereinwhen the positions of the plurality of actual obstacles are acquired bythe processor, the model prediction control is configured to calculate,for each of the plurality of actual obstacles, an evaluation valueindicating a probability in which a collision with the moving body mayoccur, and decides which of the plurality of actual obstacles is theactual obstacle to be avoided based on the evaluation value calculatedfor each of the actual obstacles.
 11. The control device according toclaim 1, wherein when there are two or three actual obstacles to beavoided, the obstacle avoidance control is configured to decide theposition of the virtual obstacle to a position which is equidistant fromthe two or three actual obstacles and has the shortest distance to eachof the actual obstacles.
 12. The control device according to claim 1,wherein the model prediction control is further is configured todetermine whether or not the moving body comes into a speed reducingstate, and the obstacle avoidance control is configured to startoperations when the moving body is determined to come into the speedreducing state under a situation that there are the plurality of actualobstacles to be avoided by the model prediction control.
 13. The controldevice according to claim 12, wherein when an absolute value of adifference between a current value and a previous value of a minimumvalue of the evaluation function, or an absolute value of a differencebetween a current value and a previous value of the position of themoving body is less than a prescribed threshold value, the modelprediction control is further configured to determine that the movingbody comes into the speed reducing state.
 14. The control deviceaccording to claim 13, wherein when the absolute value of the differencebetween the current value and the previous value of the minimum value ofthe evaluation function, or the absolute value of the difference betweenthe current value and the previous value of the position of the movingbody is less than the prescribed threshold value for a prescribed numberof times continuously, the model prediction control is furtherconfigured to determine that the moving body comes into the speedreducing state.